 |
AVR1636 Configurable PMSM Sensorless FOC using the XMEGA
1.0
|
|
AVR1636 Configurable PMSM Sensorless FOC using the XMEGA
1.0
|
00001 /* This file has been prepared for Doxygen automatic documentation generation.*/ 00062 #include <stdint.h> 00063 #include <avr/io.h> 00064 #include <avr/pgmspace.h> 00065 #include <avr/interrupt.h> 00066 #include <avr/eeprom.h> 00067 00068 00070 // Sine Look-up Table 00072 00073 //sine(0 Degrees) to sine(360 Degrees) 256 values 00074 const int8_t sine_table[256] PROGMEM = 00075 { 00076 0,3,6,9,12,15,18,21, 00077 24,27,30,33,36,39,42,45, 00078 48,51,54,57,59,62,65,67, 00079 70,73,75,78,80,82,85,87, 00080 89,91,94,96,98,100,102,103, 00081 105,107,108,110,112,113,114,116, 00082 117,118,119,120,121,122,123,123, 00083 124,125,125,126,126,126,126,126, 00084 127,126,126,126,126,126,125,125, 00085 124,123,123,122,121,120,119,118, 00086 117,116,114,113,112,110,108,107, 00087 105,103,102,100,98,96,94,91, 00088 89,87,85,82,80,78,75,73, 00089 70,67,65,62,59,57,54,51, 00090 48,45,42,39,36,33,30,27, 00091 24,21,18,15,12,9,6,3, 00092 0,-4,-7,-10,-13,-16,-19,-22, 00093 -25,-28,-31,-34,-37,-40,-43,-46, 00094 -49,-52,-55,-58,-60,-63,-66,-68, 00095 -71,-74,-76,-79,-81,-83,-86,-88, 00096 -90,-92,-95,-97,-99,-101,-103,-104, 00097 -106,-108,-109,-111,-113,-114,-115,-117, 00098 -118,-119,-120,-121,-122,-123,-124,-124, 00099 -125,-126,-126,-127,-127,-127,-127,-127, 00100 -127,-127,-127,-127,-127,-127,-126,-126, 00101 -125,-124,-124,-123,-122,-121,-120,-119, 00102 -118,-117,-115,-114,-113,-111,-109,-108, 00103 -106,-104,-103,-101,-99,-97,-95,-92, 00104 -90,-88,-86,-83,-81,-79,-76,-74, 00105 -71,-68,-66,-63,-60,-58,-55,-52, 00106 -49,-46,-43,-40,-37,-34,-31,-28, 00107 -25,-22,-19,-16,-13,-10,-7,-4 00108 }; 00109 00110 00111 //***************************************************************************** 00112 // Declarations 00113 //***************************************************************************** 00114 00115 // PWM and Duty Cycle control 00116 #define PWM_PRESCALER 4 //PWM clock prescaler 00117 #define PWM_MAX 256 //100% Duty Cycle 00118 #define PWM_HALF 127 //50% Duty Cycle 00119 #define PWM_DB 24 // Deadband 16 = 500ns 00120 00121 #define PWM_TIMER_ISR_VECTOR TCC0_CCD_vect 00122 00123 #define PWM_CHANNEL_A TCC0_CCABUF 00124 #define PWM_CHANNEL_B TCC0_CCBBUF 00125 #define PWM_CHANNEL_C TCC0_CCCBUF 00126 #define PWM_CHANNEL_D TCC0_CCDBUF 00127 00128 00129 // Limits on Regulators 00130 #define V_MAX 123 // PWM limit 127 - 4 for deadtime = 123 00131 #define V_MAX_SQUARED (V_MAX*V_MAX) 00132 #define I_MAX 120 // Current limit 00133 00134 // Oscillator Calibration value 00135 #define PWM_PERIODS 960 //30ms/31.25us 00136 00137 // UART pin definitions 00138 #define UART_DATA_REGISTER USARTD0_DATA 00139 #define UART_STATUS_RECIEVE_TEST (USARTD0_STATUS & USART_RXCIF_bm) 00140 00141 #define TXD_PIN PIN3_bm //PD3 00142 #define TxD_pin_0 PORTD_OUTCLR = TXD_PIN 00143 #define TxD_pin_1 PORTD_OUTSET = TXD_PIN 00144 #define TxD_pin_out PORTD_DIRSET |= TXD_PIN 00145 #define TxD_pin_totem PORTD_PIN3CTRL |= PORT_OPC_TOTEM_gc 00146 00147 #define RXD_PIN PIN2_bm 00148 #define RXD_pin_test (PORTD_IN & RXD_PIN) // Test RXD pin 00149 #define RXD_PIN_0 0 // Test for pin low 00150 #define RXD_PIN_1 RXD_PIN // Test for pin high 00151 00152 // Test Pin 00153 #define TEST_PIN PIN6_bm //PC6 00154 #define TEST_pin_lo PORTC_OUTCLR = TEST_PIN 00155 #define TEST_pin_hi PORTC_OUTSET = TEST_PIN 00156 #define TEST_pin_out PORTC_DIRSET |= TEST_PIN 00157 #define TEST_pin_totem PORTC_PIN6CTRL |= PORT_OPC_TOTEM_gc 00158 00159 // ADC Channels - Internal 1.1V voltage reference 00160 #define ADC_PRESCALER 4 //divide by 16 (1MHz ADC clock) 00161 #define ADC_MUX_bm 0x3F // lower 6 bits 00162 00163 // X - unused channels for user application code 00164 #define ADC_MUX_X 0 00165 // Vbus = PA6/ADC6 00166 #define ADC_MUX_VBUS (ADC_CH_MUXPOS2_bm | ADC_CH_MUXPOS1_bm) 00167 // I = PA4/ADC4 00168 #define ADC_MUX_I (ADC_CH_MUXPOS2_bm) 00169 00170 // Current Reconstruction parameters 00171 #define HALF_SCALE 128 00172 #define TRIGGER_OFFSET 1 // 1 (0.125us) 00173 #define DEAD_TIME (PWM_DB/PWM_PRESCALER) // 6 (750us) 00174 #define CURRENT_SETTLING_TIME 14 // 14 (1.75us) 00175 #define SAMPLE_HOLD_TIME 11 // 11 (1.375us) 00176 #define SAMPLE_1_OFFSET (SAMPLE_HOLD_TIME) // 11 (1.375us) 00177 #define SAMPLE_2_OFFSET (TRIGGER_OFFSET + DEAD_TIME + CURRENT_SETTLING_TIME) 00178 // 1 + 6 + 14 = 21 (1.375us) 00179 #define SAMPLE_WINDOW (SAMPLE_1_OFFSET + SAMPLE_2_OFFSET) // 11 + 21 = 32 (4us) 00180 00181 // Transformation Constants 00182 #define ONE_OVER_THREE 85 // x256 00183 #define TWO_OVER_THREE 171 // x256 00184 #define ONE_OVER_SQRT_THREE 148 // x256 00185 #define TWO_OVER_SQRT_THREE 148 // x128 00186 #define SQRT_THREE_OVER_TWO 222 // x256 00187 00188 // Variables 00189 #define VARIABLE_TABLE_SIZE 16 // number of bytes in table 00190 00191 // EEPROM 00192 #define EEPROM_TABLE_SIZE 32 // number of bytes in table 00193 00194 // EEPROM 00195 #define HEADER_SIZE 64 // number bytes 0xFFs sent between data 00196 00197 // 00198 #define MESSAGE_SIZE (HEADER_SIZE + VARIABLE_TABLE_SIZE + EEPROM_TABLE_SIZE +1) 00199 00200 00201 //***************************************************************************** 00202 // Global Variables 00203 //***************************************************************************** 00204 00205 //Loop Counter 00206 uint8_t pwm_counter = 0; // Used to count ISRs 0 to 3 for FOC functions 00207 00208 // Communication Data 00209 uint16_t byte_timeout; // UART timeout counter 00210 uint8_t data_txd; // UART data to be transmitted 00211 uint8_t data_rxd; // UART data received 00212 uint8_t data_toggle; // toggle between transmitting variable list 00213 // and high speed single variable 00214 uint8_t count_dir; // count direction for "counter" variable 00215 00216 // EEPROM Parameters 00217 uint8_t valid_byte; // Incoming UART data byte position 00218 uint8_t eeprom_number; // Current EEPROM number from UART data 00219 00220 // Motor State 00221 uint8_t motor_off; // bit 0 = motor off command 00222 // bit 1 = motor turned off, bit 2 = transfer variables back to EEPROM 00223 00224 // Current Reconstruction Parameters 00225 uint8_t i_sample_offset; // offset value for zero current 00226 uint8_t i_sample_1; // current sample 1 for current reconstruction 00227 uint8_t i_sample_2; // current sample 2 for current reconstruction 00228 uint8_t sample_state_1; // i_sample_1 pwm state, determines current measured 00229 uint8_t sample_state_2; // i_sample_2 pwm state, determines current measured 00230 uint8_t window_1; // calculated window for i_sample_1 00231 uint8_t window_2; // calculated window for i_sample_2 00232 uint8_t correction_1; // pwm correction value for i_sample_1 00233 uint8_t correction_2; // pwm correction value for i_sample_2 00234 uint8_t pwm_1; // highest duty cycle 4 = A, 2 = B, 1 = C 00235 uint8_t pwm_2; // middle duty cycle 4 = A, 2 = B, 1 = C 00236 uint8_t pwm_3; // lowest duty cycle 4 = A, 2 = B, 1 = C 00237 uint8_t pwm_a_comp; // pwm a compensated value for current sampling 00238 uint8_t pwm_b_comp; // pwm b compensated value for current sampling 00239 uint8_t pwm_c_comp; // pwm c compensated value for current sampling 00240 00241 // ADC User channel 00242 uint8_t adc_user_channel = 0; // User defines ADC channel 00243 uint8_t adc_user_sample; // Variable for user defined ADC sample 00244 00245 // Sine and Cosine Look Up 00246 int8_t sin_theta; // Value for sine of theta 00247 int8_t cos_theta; // Value for cosine of theta 00248 00249 // Stator Currents 00250 int8_t i_d; // Phase Current D Axis 00251 int8_t i_q; // Phase Current Q Axis 00252 int8_t i_alpha; // Phase Current alpha Axis 00253 int8_t i_beta; // Phase Current beta Axis 00254 int8_t i_a; // Phase Current A 00255 int8_t i_b; // Phase Current B 00256 int8_t i_c; // Phase Current C 00257 00258 int8_t i_a_s; // Sampled Phase Current A 00259 int8_t i_b_s; // Sampled Phase Current B 00260 int8_t i_c_s; // Sampled Phase Current C 00261 00262 int8_t i_a_last; // Last FOC Loop Phase Current A 00263 int8_t i_b_last; // Last FOC Loop Phase Current B 00264 int8_t i_c_last; // Last FOC Loop Phase Current C 00265 00266 // Stator Voltages 00267 int8_t v_d; // Phase Voltage D Axis 00268 int8_t v_q; // Phase Voltage Q Axis 00269 int8_t v_alpha; // Phase Voltage alpha Axis 00270 int8_t v_beta; // Phase Voltage beta Axis 00271 int8_t v_a; // Phase Voltage A 00272 int8_t v_b; // Phase Voltage B 00273 int8_t v_c; // Phase Voltage C 00274 00275 // Back EMF Variables 00276 int8_t u_a; // Phase A voltage v_bus compensated 00277 int8_t u_b; // Phase B voltage v_bus compensated 00278 int8_t u_c; // Phase C voltage v_bus compensated 00279 00280 int8_t ir_a; // Phase A I*R drop 00281 int8_t ir_b; // Phase B I*R drop 00282 int8_t ir_c; // Phase C I*R drop 00283 00284 int8_t l_didt_a; // Phase A L*di/dt 00285 int8_t l_didt_b; // Phase B L*di/dt 00286 int8_t l_didt_c; // Phase C L*di/dt 00287 00288 int8_t e_a; // Phase A back emf 00289 int8_t e_b; // Phase B back emf 00290 int8_t e_c; // Phase C back emf 00291 00292 int8_t e_alpha; // alpha Axis back emf 00293 int8_t e_beta; // beta Axis back emf 00294 00295 int8_t e_d; // D Axis back emf 00296 int8_t e_q; // Q Axis back emf 00297 00298 // PWM 00299 uint8_t pwm_a; // PWM value for Phase A 00300 uint8_t pwm_b; // PWM value for Phase B 00301 uint8_t pwm_c; // PWM value for Phase C 00302 uint8_t sample_trigger; // ADC Sample trigger value (PWM D) 00303 uint8_t pwm_state; // PWM Switching State similar to SVM vector 00304 00305 // Back EMF Phase Locked Loop Compensation 00306 int16_t p_integral; // PLL Integral Term 00307 int16_t p_min; // PLL lower frequency 00308 int16_t p_max; // PLL Upper frequency 00309 int16_t p_output_min; // PLL lower frequency with sign 00310 int16_t p_output_max; // PLL Upper frequency with sign 00311 00312 // Speed Loop Compensation 00313 uint8_t s_reg_count; 00314 int16_t s_integral; // Speed Reg Integral Term 00315 int16_t s_output_min; // Speed Reg minimum output 00316 int16_t s_output_max; // Speed Reg maximum output 00317 00318 // Current Loop Compensation 00319 int8_t i_d_ref; // D Axis current reference 00320 int8_t i_q_ref; // Q Axis current reference 00321 int16_t i_d_integral; // D Axis Current Reg Integral Term 00322 int16_t i_q_integral; // Q Axis Current Reg Integral Term 00323 int16_t i_output_max; // Current Reg maximum output 00324 00325 // General Regulator Terms 00326 int8_t input_ref; // input reference 00327 int16_t error; // regulator error 00328 int16_t comp_ki; // ki compensation 00329 int16_t output; // output of regulator 00330 00331 // Theta Update 00332 uint16_t theta_int; // Air Gap Flux Vector Angle 00333 uint8_t theta_90; // Theta + 90 for cosine look-up 00334 00335 //Speed and input_cmd 00336 int16_t speed; // Current Speed 00337 int8_t input_cmd; // input command 00338 00339 // Voltage clamp index 00340 uint16_t v_mag_squared; // v_magnitude squared 00341 00342 // Oscillator Calibration 00343 uint16_t osc_cal; // Factory Calibration Value read from registers 00344 00345 //Counter for table operations 00346 uint16_t table_counter = 0; // index for variable_table 00347 00348 //Variable and EEPROM table 00349 uint8_t variable_table[VARIABLE_TABLE_SIZE + EEPROM_TABLE_SIZE]; 00350 00351 //Aliases for variables 00352 #define v_a_display variable_table[0] 00353 #define v_b_display variable_table[1] 00354 00355 #define i_a_display variable_table[2] 00356 #define i_b_display variable_table[3] 00357 00358 #define u_a_display variable_table[4] 00359 #define ir_a_display variable_table[5] 00360 #define l_didt_a_display variable_table[6] 00361 #define e_a_display variable_table[7] 00362 00363 #define i_d_display variable_table[8] 00364 #define i_q_display variable_table[9] 00365 00366 #define e_d_display variable_table[10] 00367 00368 #define v_bus variable_table[11] 00369 #define speed_display variable_table[12] 00370 #define theta variable_table[13] 00371 00372 #define osc_error variable_table[14] 00373 00374 #define counter variable_table[15] 00375 00376 //Aliases for variables holding EEPROM values 00377 #define v_bus_min variable_table[0 + VARIABLE_TABLE_SIZE] 00378 00379 #define osc_cal_adj variable_table[1 + VARIABLE_TABLE_SIZE] 00380 00381 #define data_watch variable_table[2 + VARIABLE_TABLE_SIZE] 00382 #define mode variable_table[3 + VARIABLE_TABLE_SIZE] 00383 #define input_cmd_display variable_table[4 + VARIABLE_TABLE_SIZE] 00384 00385 #define s_reg_rate variable_table[5 + VARIABLE_TABLE_SIZE] 00386 #define s_reg_scaling variable_table[6 + VARIABLE_TABLE_SIZE] 00387 #define s_error_limit variable_table[7 + VARIABLE_TABLE_SIZE] 00388 #define s_k_scaling variable_table[8 + VARIABLE_TABLE_SIZE] 00389 #define s_kp variable_table[9 + VARIABLE_TABLE_SIZE] 00390 #define s_ki variable_table[10 + VARIABLE_TABLE_SIZE] 00391 00392 #define i_limit variable_table[11 + VARIABLE_TABLE_SIZE] 00393 00394 #define i_reg_scaling variable_table[12 + VARIABLE_TABLE_SIZE] 00395 #define i_error_limit variable_table[13 + VARIABLE_TABLE_SIZE] 00396 #define i_k_scaling variable_table[14 + VARIABLE_TABLE_SIZE] 00397 #define i_kp variable_table[15 + VARIABLE_TABLE_SIZE] 00398 #define i_ki variable_table[16 + VARIABLE_TABLE_SIZE] 00399 00400 #define v_limit variable_table[17 + VARIABLE_TABLE_SIZE] 00401 00402 #define speed_scale variable_table[18 + VARIABLE_TABLE_SIZE] 00403 #define speed_min variable_table[19 + VARIABLE_TABLE_SIZE] 00404 #define speed_max variable_table[20 + VARIABLE_TABLE_SIZE] 00405 00406 #define p_reg_scaling variable_table[21 + VARIABLE_TABLE_SIZE] 00407 #define p_error_limit variable_table[22 + VARIABLE_TABLE_SIZE] 00408 #define p_k_scaling variable_table[23 + VARIABLE_TABLE_SIZE] 00409 #define p_kp variable_table[24 + VARIABLE_TABLE_SIZE] 00410 #define p_ki variable_table[25 + VARIABLE_TABLE_SIZE] 00411 #define p_gravity variable_table[26 + VARIABLE_TABLE_SIZE] 00412 00413 #define u_scaling variable_table[27 + VARIABLE_TABLE_SIZE] 00414 #define r_scaling variable_table[28 + VARIABLE_TABLE_SIZE] 00415 #define r_phase variable_table[29 + VARIABLE_TABLE_SIZE] 00416 #define l_scaling variable_table[30 + VARIABLE_TABLE_SIZE] 00417 #define l_phase variable_table[31 + VARIABLE_TABLE_SIZE] 00418 00419 00420 //***************************************************************************** 00421 // initialize Clock to 32MHz oscillator 00422 //***************************************************************************** 00423 00434 void init_oscillator(void) 00435 { 00436 CCP = CCP_IOREG_gc; // disable register security for oscillator update 00437 OSC_CTRL = OSC_RC32MEN_bm; // enable 32MHz oscillator 00438 while(!(OSC_STATUS & OSC_RC32MRDY_bm)) 00439 { 00440 // wait for oscillator to be ready 00441 } 00442 CCP = CCP_IOREG_gc; // disable register security for clock update 00443 CLK_CTRL = CLK_SCLKSEL_RC32M_gc; // switch to 32MHz clock 00444 } 00445 00446 00447 //***************************************************************************** 00448 // initialize Test pin 00449 //***************************************************************************** 00450 00461 void init_test_pin(void) 00462 { 00463 TEST_pin_lo; 00464 TEST_pin_out; 00465 TEST_pin_totem; 00466 } 00467 00468 //***************************************************************************** 00469 // Initialize PWM 00470 //***************************************************************************** 00471 00486 void init_pwm(void) 00487 { 00488 PORTC_DIR |= 0x3F; // set as output 00489 00490 TCC0_CTRLA = 0x03; // Prescaler: clk/4 00491 00492 //Bit 7:4 – CCxEN: Compare or Capture Enable 00493 //Bit 2:0 – WGMODE[2:0]: Waveform Generation Mode = 011 SINGLESLOPE 00494 TCC0_CTRLB = 0x03; // Single-slope PWM 00495 TCC0_PER = PWM_MAX-1; // 00496 TCC0_CCA = PWM_HALF; // 00497 TCC0_CCB = PWM_HALF; // 00498 TCC0_CCC = PWM_HALF; 00499 TCC0_CCD = PWM_HALF; 00500 00501 //CTRL - Control Register 00502 //Bit 3:0 - DTICCxEN: Dead-Time Insertion CCx Enable 00503 AWEXC_CTRL = 0x07; //Enable A. B, and C dead time 00504 00505 //Dead-Time Concurrent write to Low and High Side Buffer Registers 00506 AWEXC_DTLS = PWM_DB; // 24 = 750ns 00507 AWEXC_DTHS = PWM_DB; // 24 = 750ns 00508 } 00509 00510 00511 //***************************************************************************** 00512 // initialize ADC PA3(ADC3),PA4(ADC4) 00513 //***************************************************************************** 00514 00524 void init_adc(void) 00525 { 00526 00527 // CTRLA - ADC Control Register A 00528 // Bit 0 – ENABLE: ADC Enable 00529 00530 // CTRLB - ADC Control Register B 00531 // Bit 4 - CONVMODE: ADC Conversion Mode: 0 = unsigned mode 00532 // Bits 2:1 - RESOLUTION[1:0]: ADC Conversion Result Resolution 00533 ADCA_CTRLB = ADC_RESOLUTION1_bm; //8 bit resolution 00534 00535 // REFCTRL - ADC Reference Control register 00536 // Bits 6:4 – REFSEL[2:0]:ADC Reference Selection: 000 INT1V Internal 1.00V 00537 // Bit 1 – BANDGAP: Bandgap enable 00538 00539 // PRESCALER - ADC Clock Prescaler register 00540 // Bits 2:0 - PRESCALER[2:0]: ADC Prescaler configuration: 001 DIV8 8 00541 ADCA_PRESCALER = ADC_PRESCALER_DIV32_gc; 00542 00543 // CTRL - ADC Channel Control Register 00544 // Bit 7 - START: START Conversion on Channel 00545 // Bits 4:2 - GAIN[2:0]: ADC Gain Factor 00546 // Bit 1:0 - INPUTMODE[1:0]: ADC Input Mode: 01 SINGLEENDED 00547 ADCA_CH0_CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc; 00548 00549 // MUXCTRL - ADC Channel MUX Control registers 00550 // Bits 6:3 - MUXPOS[3:0]: MUX selection on Positive ADC input 00551 // Bits 1:0 - MUXNEG[1:0]: MUX selection on Negative ADC input 00552 //current sampling 00553 ADCA_CH0_MUXCTRL = ADC_MUX_I; 00554 00555 // CTRLA - ADC Control Register A 00556 // Bit 0 – ENABLE: ADC Enable 00557 ADCA_CTRLA = ADC_ENABLE_bm; 00558 00559 // EVCTRL - ADC Event Control Register 00560 // Bits 5:3 - EVSEL[2:0]: event channel input select: 0 to 3 00561 // Bits 0 - EVACT0: ADC Event Mode 00562 //Enable Event trigger 00563 ADCA_EVCTRL |= ADC_EVACT0_bm; 00564 00565 } 00566 00567 //***************************************************************************** 00568 // Initialize UART - USARTD0 00569 //***************************************************************************** 00570 00581 void init_uart(void) 00582 { 00583 uint16_t bsel = 1047; 00584 uint8_t bscale = 10; 00585 // USART initialization should use the following sequence: 00586 // 1. Set the TxD pin value high, and optionally the XCK pin low. 00587 // 2. Set the TxD and optionally the XCK pin as output. 00588 // 3. Set the baud rate and frame format. 00589 // 4. Set mode of operation (enables XCK pin output in synchronous mode). 00590 // 5. Enable the Transmitter or the Receiver depending on the usage. 00591 00592 TxD_pin_1; 00593 TxD_pin_out; 00594 00595 // DATA - USART I/O Data Register 00596 // USARTD0_DATA 00597 00598 // 115200 @ 32Mhz as calculated from ProtoTalk Calc 00599 // USARTD0_BAUDCTRLA = (uint8_t) bsel; 00600 USARTD0_BAUDCTRLA = (0xFF & bsel); 00601 USARTD0_BAUDCTRLB = (bscale << 4) | (bsel >> 8); 00602 00603 // STATUS - USART Status Register 00604 // USART_RXCIF_bm 0x80 /* Receive Interrupt Flag bit mask. */ 00605 // USART_TXCIF_bm 0x40 /* Transmit Interrupt Flag bit mask. */ 00606 // USARTD0_STATUS 00607 00608 // BAUDCTRLA - USART Baud Rate Register 00609 // Bit 7:0 - BSEL[7:0]: USART Baud Rate Register 00610 // USARTD0_BAUDCTRLA = 138; //For 115200 Baud, 32MHz clock, BSCALE = 10 00611 00612 // BAUDCTRLB - USART Baud Rate Register 00613 // Bit 7:4 - BSCALE[3:0]: USART Baud Rate Scale factor 00614 // Bit 3:0 - BSEL[11:8]: USART Baud Rate Register 00615 // USARTD0_BAUDCTRLB = 00616 // ( USART_BSCALE3_bm | USART_BSCALE2_bm | USART_BSCALE0_bm); 00617 00618 // CTRLA – USART Control Register A 00619 // Interrupt Level - Unused 00620 USARTD0_CTRLA = 0; 00621 00622 // CTRLC - USART Control Register C 00623 // Bits 7:6 - CMODE[1:0]: USART Communication Mode: 00 = Asynchronous USART 00624 // Bit 2:0 - CHSIZE[2:0]: Character Size: 011 = 8BIT, 8-bit 00625 USARTD0_CTRLC = USART_CHSIZE_8BIT_gc; 00626 00627 // CTRLB - USART Control Register B 00628 // Bit 4 - RXEN: Receiver Enable 00629 // USART_RXEN_bm 0x10 /* Receiver Enable bit mask. */ 00630 // Bit 3 - TXEN: Transmitter Enable 00631 // USART_TXEN_bm 0x08 /* Transmitter Enable bit mask. */ 00632 USARTD0_CTRLB = (USART_RXEN_bm | USART_TXEN_bm); 00633 00634 } 00635 00636 00637 //***************************************************************************** 00638 // Initalize Event System 00639 //***************************************************************************** 00640 00652 void init_eventsys(void) 00653 { 00654 // CHnMUX – Event Channel n Multiplexer Register 00655 // 1100 1100 TCxn_CCA Capture or Compare D Timer/Counter C0 event type E 00656 EVSYS_CH0MUX = 0xC7; 00657 00658 // CLKEVOUT - Clock and Event Out Register 00659 // Bit 5:4 - EVOUT[1:0] - Event Output Port: 00660 // 10 = PD7 Event Channel 0 output on Port D pin 7 00661 PORTCFG_CLKEVOUT |= PORTCFG_EVOUT1_bm; 00662 00663 PORTD_DIRSET |= PIN7_bm; 00664 } 00665 00666 //***************************************************************************** 00667 // Read in EEPROM values 00668 //***************************************************************************** 00669 00679 void read_eeprom(void) 00680 { 00681 cli(); 00682 table_counter = 0; 00683 while (table_counter < EEPROM_TABLE_SIZE) 00684 { 00685 variable_table[VARIABLE_TABLE_SIZE + table_counter] = 00686 eeprom_read_byte((uint8_t*)(table_counter)); 00687 table_counter++; 00688 } 00689 table_counter = 0; 00690 sei(); 00691 } 00692 00693 00694 //***************************************************************************** 00695 // write EEPROM values 00696 //***************************************************************************** 00697 00707 void write_eeprom(void) 00708 { 00709 cli(); 00710 table_counter = 0; 00711 while (table_counter < EEPROM_TABLE_SIZE) 00712 { 00713 eeprom_write_byte ((uint8_t*)(table_counter), 00714 variable_table[VARIABLE_TABLE_SIZE + table_counter]); 00715 table_counter++; 00716 } 00717 table_counter = 0; 00718 motor_off = 0; 00719 sei(); 00720 } 00721 00722 00723 //***************************************************************************** 00724 // read oscillator calibration 00725 //***************************************************************************** 00726 00734 void read_oscillator_cal(void) 00735 { 00736 osc_cal = ((DFLLRC32M_CALB & 0x1F)<<8) + DFLLRC32M_CALA; 00737 } 00738 00739 00740 //***************************************************************************** 00741 // write oscillator calibration 00742 //***************************************************************************** 00743 00751 void write_oscillator_cal(void) 00752 { 00753 if (osc_cal_adj < 65) 00754 { 00755 DFLLRC32M_CALB = (((osc_cal + osc_cal_adj - 32)>>8) & 0x1F); 00756 DFLLRC32M_CALA = ((osc_cal + osc_cal_adj - 32) & 0xFF); 00757 } 00758 } 00759 00760 //***************************************************************************** 00761 // test oscillator calibration 00762 //***************************************************************************** 00763 00774 void test_oscillator(void) 00775 { 00776 uint16_t osc_pwm_periods; 00777 cli(); 00778 osc_pwm_periods = 0; 00779 while ((PORTD_IN & RXD_PIN) == RXD_PIN_1 ) 00780 { 00781 00782 } 00783 while ((PORTD_IN & RXD_PIN) == RXD_PIN_0 ) 00784 { 00785 if((TCC0_INTFLAGS & TC0_CCDIF_bm) == TC0_CCDIF_bm) 00786 { 00787 TCC0_INTFLAGS |= TC0_CCDIF_bm; 00788 osc_pwm_periods++; 00789 } 00790 } 00791 osc_error = 128 + osc_pwm_periods - PWM_PERIODS; 00792 motor_off = 0; 00793 table_counter = 0; 00794 sei(); 00795 } 00796 00797 00798 //***************************************************************************** 00799 // Current Sense Calibration 00800 //***************************************************************************** 00801 00811 void calibrate_current(void) 00812 { 00813 uint8_t bit_test; 00814 AWEXC_OUTOVEN = 0x00; //Make sure all PWM outputs are disabled 00815 00816 while (i_sample_offset == 0) 00817 { 00818 cli(); 00819 bit_test = pwm_counter; 00820 sei(); 00821 if (bit_test == 3) 00822 { 00823 TEST_pin_hi; 00824 cli(); 00825 if ((i_sample_1 > 10) && (i_sample_1 == i_sample_2)) 00826 { 00827 i_sample_offset = HALF_SCALE - i_sample_1; 00828 } 00829 sei(); 00830 } 00831 } 00832 AWEXC_OUTOVEN = 0x3F; //Enable all PWM outputs 00833 } 00834 00835 00836 //***************************************************************************** 00837 // Enable Interrupts 00838 //***************************************************************************** 00839 00848 void init_interrupts(void) 00849 { 00850 cli(); 00851 TCC0_INTCTRLB |= (TC0_CCDINTLVL1_bm | TC0_CCDINTLVL0_bm); 00852 PMIC_CTRL |= (PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm); 00853 sei(); 00854 } 00855 00856 00857 //***************************************************************************** 00858 // PWM Timer Interrupt - ADC Sampling, Current Reconstruction, and UART 00859 //***************************************************************************** 00860 00868 ISR(PWM_TIMER_ISR_VECTOR) 00869 { 00870 //TEST_pin_hi; 00871 00872 // Motor Off function for updating EEPROM 00873 if (v_bus < v_bus_min) 00874 { 00875 input_ref = 0; 00876 } 00877 else 00878 { 00879 input_ref = input_cmd; 00880 } 00881 00882 if (motor_off > 0) 00883 { 00884 motor_off |= 2; //Motor PWM at 50% 00885 pwm_a_comp = PWM_HALF; 00886 pwm_b_comp = PWM_HALF; 00887 pwm_c_comp = PWM_HALF; 00888 sample_trigger = PWM_HALF; 00889 } 00890 00892 // Clamp on Sample Trigger 00894 00895 if (sample_trigger >= PWM_MAX) 00896 { 00897 sample_trigger = PWM_MAX -1; 00898 } 00899 if (sample_trigger == 0) 00900 { 00901 sample_trigger = 1; 00902 } 00903 00904 00906 // Three Phase PWM and Sample Trigger Update 00908 00909 PWM_CHANNEL_A = PWM_MAX - pwm_a_comp; //Duty cycle 00910 PWM_CHANNEL_B = PWM_MAX - pwm_b_comp; //Duty cycle 00911 PWM_CHANNEL_C = PWM_MAX - pwm_c_comp; //Duty cycle 00912 PWM_CHANNEL_D = PWM_MAX - sample_trigger; //Duty cycle 00913 00914 00916 // Update EEPROM 00918 00919 // Write EEPROM function here. Non-reentrant so must disable interrupts 00920 // bit 0 = motor off command, bit 1 = motor turned off, 00921 // bit 2 = transfer variables back to EEPROM, bit 3 = Cal Oscillator 00922 if (motor_off == 7) 00923 { 00924 write_eeprom(); 00925 } 00926 00927 00929 // Test Oscillator 00931 00932 // bit 0 = motor off command, bit 1 = motor turned off, 00933 // bit 2 = transfer variables back to EEPROM bit 3 = Cal Oscillator 00934 if (motor_off == 11) 00935 { 00936 write_oscillator_cal(); 00937 test_oscillator(); 00938 } 00939 00940 00942 // PWM Counter Update 00944 00945 pwm_counter++; 00946 if (pwm_counter > 3) 00947 { 00948 pwm_counter = 0; 00949 } 00950 00951 00953 // Serial Communications Update 00955 00956 if (pwm_counter == 0) 00957 { 00958 if (data_toggle) 00959 { 00960 if (table_counter < (VARIABLE_TABLE_SIZE + EEPROM_TABLE_SIZE)) 00961 { 00962 data_txd = variable_table[table_counter]; 00963 } 00964 else 00965 { 00966 if (table_counter == (VARIABLE_TABLE_SIZE + EEPROM_TABLE_SIZE)) 00967 { 00968 data_txd = 0; //0 at end of data 00969 } 00970 else 00971 { 00972 data_txd = 255; //The rest of the data is 255s 00973 } 00974 00975 } 00976 table_counter++; 00977 if (table_counter > MESSAGE_SIZE) 00978 { 00979 table_counter = 0; 00980 data_txd = 0; //0 at beginning of data 00981 } 00982 UART_DATA_REGISTER = data_txd; 00983 data_toggle = 0; 00984 } 00985 else 00986 { 00987 00988 if (count_dir == 0) 00989 { 00990 counter++; 00991 if (counter>254) 00992 { 00993 count_dir = 1; 00994 } 00995 } 00996 else 00997 { 00998 counter--; 00999 if (counter<1) 01000 { 01001 count_dir = 0; 01002 } 01003 } 01004 01005 data_txd = variable_table[data_watch]; 01006 if (data_txd > 254) 01007 { 01008 data_txd = 254; // Clip data 01009 } 01010 UART_DATA_REGISTER = data_txd; 01011 data_toggle = 1; 01012 } 01013 } 01014 01015 byte_timeout++; 01016 01017 // if (input_state == 1) 01018 { 01019 01020 if (UART_STATUS_RECIEVE_TEST) // Receive buffer full? 01021 { 01022 data_rxd = UART_DATA_REGISTER; // valid start bit 01023 valid_byte++; 01024 } 01025 01026 01027 if ( byte_timeout >= 32500) // 32500/15625Hz/2 = 1sec 01028 { 01029 valid_byte=0; 01030 byte_timeout=0; 01031 } 01032 01033 if (valid_byte == 3) 01034 { 01035 if (eeprom_number < EEPROM_TABLE_SIZE) 01036 { 01037 variable_table[eeprom_number + VARIABLE_TABLE_SIZE] = data_rxd; 01038 } 01039 01040 valid_byte=0; 01041 if ((eeprom_number == 254)&(data_rxd == 254)) 01042 { 01043 // Motor off command and variables ready for write 01044 motor_off = 5; 01045 eeprom_number = 0; 01046 } 01047 if ((eeprom_number == 255)&(data_rxd == 255)) 01048 { 01049 // Motor off command and calibrate oscillator 01050 motor_off = 9; 01051 eeprom_number = 0; 01052 } 01053 } 01054 01055 if (valid_byte == 1) 01056 { 01057 eeprom_number = data_rxd; 01058 valid_byte++; 01059 } 01060 } 01061 01063 // Limit on Scaling variables (0 to 7) 01065 01066 speed_scale = speed_scale & 0x07; 01067 01068 p_reg_scaling = p_reg_scaling & 0x07; 01069 p_k_scaling = p_k_scaling & 0x07; 01070 01071 s_reg_scaling = s_reg_scaling & 0x07; 01072 s_k_scaling = s_k_scaling & 0x07; 01073 01074 i_reg_scaling = i_reg_scaling & 0x07; 01075 i_k_scaling = i_k_scaling & 0x07; 01076 01077 u_scaling = u_scaling & 0x07; 01078 01079 r_scaling = r_scaling & 0x07; 01080 01081 l_scaling = l_scaling & 0x07; 01082 01083 if (v_limit > V_MAX) 01084 { 01085 v_limit = V_MAX; 01086 } 01087 if (i_limit > I_MAX) 01088 { 01089 i_limit = I_MAX; 01090 } 01091 01092 01093 01095 // Sensorless FOC Routines (Cycles through 1 of 4 each ISR) 01097 01098 switch (pwm_counter) 01099 { 01100 01101 case 0: // 01102 // TEST_pin_hi; 01103 01105 // Update Angle 01107 01108 theta_int += speed; 01109 01110 theta = (theta_int>>speed_scale) & 0xFF; 01111 01112 // 90 Degrees = 256/360*90=64 01113 theta_90 = (theta+64) & 0xFF; 01114 01115 if ((theta < 64) || (theta > 192)) 01116 { 01117 TEST_pin_hi; 01118 } 01119 else 01120 { 01121 TEST_pin_lo; 01122 } 01123 01124 01126 // Sine and Cosine Lookup 01128 01129 sin_theta = pgm_read_byte(&(sine_table[theta])); 01130 cos_theta = pgm_read_byte(&(sine_table[theta_90])); 01131 01132 01134 // Phase Current Updates 01136 01137 // Phase Currents from last loop update 01138 i_a_last = i_a; 01139 i_b_last = i_b; 01140 i_c_last = i_c; 01141 01142 // Phase Currents out to main loop 01143 i_a = i_a_s; //0.075A/count 01144 i_b = i_b_s; 01145 i_c = i_c_s; 01146 01147 01149 // Speed and Current Limit Update 01151 01152 //Recalc limits in case they have changed from Config Utility 01153 p_min = (speed_min<<p_reg_scaling); 01154 p_max = (speed_max<<p_reg_scaling); 01155 01156 s_output_max = (i_limit<<s_reg_scaling); // Calculated max current 01157 01158 i_output_max = (v_limit<<i_reg_scaling); 01159 01160 01162 // Display Variables Update 01164 01165 v_a_display = v_a + HALF_SCALE; 01166 v_b_display = v_b + HALF_SCALE; 01167 01168 i_a_display = i_a_s + HALF_SCALE; 01169 i_b_display = i_b_s + HALF_SCALE; 01170 01171 u_a_display = u_a + HALF_SCALE; 01172 ir_a_display = ir_a + HALF_SCALE; 01173 01174 l_didt_a_display = l_didt_a + HALF_SCALE; 01175 e_a_display = e_a + HALF_SCALE; 01176 01177 i_d_display = i_d + HALF_SCALE; 01178 i_q_display = i_q + HALF_SCALE; 01179 01180 e_d_display = e_d + HALF_SCALE; 01181 01182 input_cmd = input_cmd_display - HALF_SCALE; 01183 01184 //Speed Display uses half value to fit signed number on Variable plot 01185 speed_display = speed + HALF_SCALE; 01186 01187 01189 // Speed Regulator 01191 01192 if (mode != 2) 01193 { 01194 s_integral = 0; 01195 } 01196 s_reg_count++; 01197 if (s_reg_count >= s_reg_rate) 01198 { 01199 s_reg_count = 0; 01200 01201 error = input_ref - speed; 01202 01203 if(error>s_error_limit) 01204 { 01205 error=s_error_limit; 01206 } 01207 if(error<-s_error_limit) 01208 { 01209 error=-s_error_limit; 01210 } 01211 01212 output = (error * s_kp)>>s_k_scaling; //Kp 01213 01214 comp_ki = (error * s_ki)>>s_k_scaling; // Ki 01215 01216 s_integral += comp_ki; 01217 01218 if (s_integral < -s_output_max) 01219 { 01220 s_integral = -s_output_max; 01221 } 01222 if (s_integral > s_output_max) 01223 { 01224 s_integral = s_output_max; 01225 } 01226 01227 output += s_integral; 01228 01229 if (output < -s_output_max) 01230 { 01231 output = -s_output_max; 01232 } 01233 if (output > s_output_max) 01234 { 01235 output = s_output_max; 01236 } 01237 01238 i_d_ref = 0; 01239 i_q_ref = output>>s_reg_scaling; 01240 } 01241 01242 01244 // ADC Sample 01246 01247 adc_user_sample = ADCA_CH0_RES; 01248 ADCA_CH0_MUXCTRL = ADC_MUX_VBUS; 01249 01250 01252 // Sample 1 PWM correction 01254 01255 switch (pwm_1) 01256 { 01257 case 1: // c (001) 1 01258 pwm_a_comp = pwm_a; 01259 pwm_b_comp = pwm_b; 01260 pwm_c_comp = pwm_c + correction_1; 01261 break; 01262 01263 case 2: // b (010) 2 01264 pwm_a_comp = pwm_a; 01265 pwm_b_comp = pwm_b + correction_1; 01266 pwm_c_comp = pwm_c; 01267 break; 01268 01269 case 4: // a (100) 4 01270 pwm_a_comp = pwm_a + correction_1; 01271 pwm_b_comp = pwm_b; 01272 pwm_c_comp = pwm_c; 01273 break; 01274 01275 default: 01276 pwm_a_comp = PWM_HALF; 01277 pwm_b_comp = PWM_HALF; 01278 pwm_c_comp = PWM_HALF; 01279 break; 01280 } 01281 01282 switch (pwm_2) 01283 { 01284 case 1: // c (001) 1 01285 sample_trigger = pwm_c_comp + SAMPLE_1_OFFSET; 01286 break; 01287 01288 case 2: // b (010) 2 01289 sample_trigger = pwm_b_comp + SAMPLE_1_OFFSET; 01290 break; 01291 01292 case 4: // a (100) 4 01293 sample_trigger = pwm_a_comp + SAMPLE_1_OFFSET; 01294 break; 01295 01296 default: 01297 sample_trigger = PWM_HALF; 01298 break; 01299 } 01300 01301 01302 //TEST_pin_lo; 01303 break; 01304 01305 case 1: // 01306 //TEST_pin_hi; 01307 01309 // Back EMF Calculation 01311 01312 // Phase voltage 01313 u_a = (v_a * v_bus)>>u_scaling; // 0.06V/count 01314 u_b = (v_b * v_bus)>>u_scaling; 01315 u_c = (v_c * v_bus)>>u_scaling; 01316 01317 // r_phase is 0.00625 Ohms/count 01318 // 0.225 Ohms/0.00625 ohms/count = 36 01319 01320 ir_a = (i_a * r_phase)>>r_scaling; // 0.06V/count 01321 ir_b = (i_b * r_phase)>>r_scaling; 01322 ir_c = (i_c * r_phase)>>r_scaling; 01323 01324 l_didt_a = ((i_a - i_a_last) * l_phase)>>l_scaling; // 0.06V/count 01325 l_didt_b = ((i_b - i_b_last) * l_phase)>>l_scaling; 01326 l_didt_c = ((i_c - i_c_last) * l_phase)>>l_scaling; 01327 01328 e_a = u_a - ir_a - l_didt_a; 01329 e_b = u_b - ir_b - l_didt_b; 01330 e_c = u_c - ir_c - l_didt_c; 01331 01332 01334 // Current 3 => 2 Clarke Transform 01336 01337 i_alpha =i_a; 01338 i_beta=(((i_a*ONE_OVER_SQRT_THREE)/2)+i_b*TWO_OVER_SQRT_THREE)/128; 01339 01340 01342 // Current Vector Rotation - Park Transform 01344 01345 i_d = (((i_alpha * cos_theta)) + ((i_beta * sin_theta)))/128; 01346 i_q = (-((i_alpha * sin_theta)) + ((i_beta * cos_theta)))/128; 01347 01348 01350 // Current Command 01352 01353 if (mode == 0) 01354 { 01355 i_d_ref = 0; // Field Weakening not implemented 01356 i_q_ref = 0; 01357 i_d_integral = 0; 01358 i_q_integral = 0; 01359 } 01360 if (mode == 1) 01361 { 01362 i_d_ref = 0; // Field Weakening not implemented 01363 i_q_ref = input_ref; 01364 if ( input_ref > i_limit) 01365 { 01366 i_q_ref = i_limit; 01367 } 01368 if ( input_ref < -i_limit) 01369 { 01370 i_q_ref = -i_limit; 01371 } 01372 if (input_ref == 0) 01373 { 01374 i_d_integral = 0; 01375 i_q_integral = 0; 01376 } 01377 } 01378 01379 01381 // ADC Sample - v_bus 01383 01384 v_bus = ADCA_CH0_RES; 01385 ADCA_CH0_MUXCTRL = ADC_MUX_I; 01386 01387 01389 // Sample 2 PWM correction 01391 01392 switch (pwm_3) 01393 { 01394 case 1: // c (001) 1 01395 pwm_a_comp = pwm_a; 01396 pwm_b_comp = pwm_b; 01397 pwm_c_comp = pwm_c - correction_2; 01398 break; 01399 01400 case 2: // b (010) 2 01401 pwm_a_comp = pwm_a; 01402 pwm_b_comp = pwm_b - correction_2; 01403 pwm_c_comp = pwm_c; 01404 break; 01405 01406 case 4: // a (100) 4 01407 pwm_a_comp = pwm_a - correction_2; 01408 pwm_b_comp = pwm_b; 01409 pwm_c_comp = pwm_c; 01410 break; 01411 01412 default: 01413 pwm_a_comp = PWM_HALF; 01414 pwm_b_comp = PWM_HALF; 01415 pwm_c_comp = PWM_HALF; 01416 break; 01417 } 01418 01419 switch (pwm_2) 01420 { 01421 case 1: // c (001) 1 01422 sample_trigger = pwm_c_comp - SAMPLE_2_OFFSET; 01423 break; 01424 01425 case 2: // b (010) 2 01426 sample_trigger = pwm_b_comp - SAMPLE_2_OFFSET; 01427 break; 01428 01429 case 4: // a (100) 4 01430 sample_trigger = pwm_a_comp - SAMPLE_2_OFFSET; 01431 break; 01432 01433 default: 01434 sample_trigger = PWM_HALF; 01435 break; 01436 } 01437 01438 01439 //TEST_pin_lo; 01440 break; 01441 01442 case 2: // 01443 //TEST_pin_hi; 01445 // Clarke Transform 3 => 2 Back EMF 01447 01448 e_alpha =e_a; 01449 e_beta=(((e_a*ONE_OVER_SQRT_THREE)/2)+e_b*TWO_OVER_SQRT_THREE)/128; 01450 01451 01453 // Park Transform - Vector Rotation - Back EMF 01455 01456 e_d = (((e_alpha * cos_theta)) + ((e_beta * sin_theta)))/128; 01457 e_q = (-((e_alpha * sin_theta)) + ((e_beta * cos_theta)))/128; 01458 01459 01461 // i_d regulator 01463 01464 error = i_d_ref - i_d; 01465 01466 if(error>i_error_limit) 01467 { 01468 error=i_error_limit; 01469 } 01470 if(error<-i_error_limit) 01471 { 01472 error=-i_error_limit; 01473 } 01474 01475 output = (error*i_kp)>>i_k_scaling; //Kp 01476 01477 comp_ki = (error * i_ki)>>i_k_scaling; // Ki 01478 01479 i_d_integral += comp_ki; 01480 01481 if (i_d_integral < -i_output_max) 01482 { 01483 i_d_integral = -i_output_max; 01484 } 01485 if (i_d_integral > i_output_max) 01486 { 01487 i_d_integral = i_output_max; 01488 } 01489 01490 output += i_d_integral; 01491 01492 if (output < -i_output_max) 01493 { 01494 output = -i_output_max; 01495 } 01496 if (output > i_output_max) 01497 { 01498 output = i_output_max; 01499 } 01500 01501 v_d = output>>i_reg_scaling; 01502 01503 01505 // i_q regulator 01507 01508 error = i_q_ref - i_q; 01509 01510 if(error>i_error_limit) 01511 { 01512 error=i_error_limit; 01513 } 01514 if(error<-i_error_limit) 01515 { 01516 error=-i_error_limit; 01517 } 01518 01519 output = (error * i_kp)>>i_k_scaling; //Kp 01520 01521 comp_ki = (error * i_ki)>>i_k_scaling; // Ki 01522 01523 i_q_integral += comp_ki; 01524 01525 if (i_q_integral < -i_output_max) 01526 { 01527 i_q_integral = -i_output_max; 01528 } 01529 if (i_q_integral > i_output_max) 01530 { 01531 i_q_integral = i_output_max; 01532 } 01533 01534 output += i_q_integral; 01535 01536 if (output < -i_output_max) 01537 { 01538 output = -i_output_max; 01539 } 01540 if (output > i_output_max) 01541 { 01542 output = i_output_max; 01543 } 01544 01545 v_q = output>>i_reg_scaling; 01546 01547 01549 // Voltage Command 01551 01552 01553 if (mode == 0) 01554 { 01555 v_d = 0; 01556 v_q = input_ref; 01557 if ( input_ref > v_limit) 01558 { 01559 v_q = v_limit; 01560 } 01561 if ( input_ref < -v_limit) 01562 { 01563 v_q = -v_limit; 01564 } 01565 } 01566 01567 01569 // Voltage Clamp 01571 01572 v_mag_squared = (v_d * v_d) + (v_q * v_q); 01573 01574 // Test for outside the unit circle 01575 // if true put in voltage mode 01576 if (v_mag_squared > V_MAX_SQUARED) 01577 { 01578 v_d = 0; 01579 if (v_q > 0) 01580 { 01581 v_q = v_limit; 01582 } 01583 else 01584 { 01585 v_q = -v_limit; 01586 } 01587 } 01588 01589 01591 // ADC Sample 01593 01594 i_sample_1 = ADCA_CH0_RES + i_sample_offset; 01595 ADCA_CH0_MUXCTRL = ADC_MUX_I; 01596 01598 // Current 1 Sample Calculations 01600 01601 switch (sample_state_1) 01602 { 01603 case 1: // V(001) = I_c 01604 i_c_s = i_sample_1 - HALF_SCALE; 01605 break; 01606 01607 case 2: // V(010) = I_b 01608 i_b_s = i_sample_1 - HALF_SCALE; 01609 break; 01610 01611 case 3: // V(011) = -I_a 01612 i_a_s = HALF_SCALE - i_sample_1; 01613 break; 01614 01615 case 4: // V(100) = I_a 01616 i_a_s = i_sample_1 - HALF_SCALE; 01617 break; 01618 01619 case 5: // V(101) = -I_b 01620 i_b_s = HALF_SCALE - i_sample_1; 01621 break; 01622 01623 case 6: // V(110) = -I_c 01624 i_c_s = HALF_SCALE - i_sample_1; 01625 break; 01626 } 01627 01628 01630 // Sample 2 PWM "anti-correction" 01632 01633 switch (pwm_3) 01634 { 01635 case 1: // c (001) 1 01636 pwm_a_comp = pwm_a; 01637 pwm_b_comp = pwm_b; 01638 pwm_c_comp = pwm_c + correction_2; 01639 break; 01640 01641 case 2: // b (010) 2 01642 pwm_a_comp = pwm_a; 01643 pwm_b_comp = pwm_b + correction_2; 01644 pwm_c_comp = pwm_c; 01645 break; 01646 01647 case 4: // a (100) 4 01648 pwm_a_comp = pwm_a + correction_2; 01649 pwm_b_comp = pwm_b; 01650 pwm_c_comp = pwm_c; 01651 break; 01652 01653 default: 01654 pwm_a_comp = PWM_HALF; 01655 pwm_b_comp = PWM_HALF; 01656 pwm_c_comp = PWM_HALF; 01657 break; 01658 } 01659 01660 switch (pwm_2) 01661 { 01662 case 1: // c (001) 1 01663 sample_trigger = pwm_c_comp - SAMPLE_2_OFFSET; 01664 break; 01665 01666 case 2: // b (010) 2 01667 sample_trigger = pwm_b_comp - SAMPLE_2_OFFSET; 01668 break; 01669 01670 case 4: // a (100) 4 01671 sample_trigger = pwm_a_comp - SAMPLE_2_OFFSET; 01672 break; 01673 01674 default: 01675 sample_trigger = PWM_HALF; 01676 break; 01677 } 01678 01679 01680 //TEST_pin_lo; 01681 break; 01682 01683 case 3: // 01684 //TEST_pin_hi; 01686 // Back EMF PLL 01688 01689 if (v_q == 0) 01690 { 01691 p_integral = 0; 01692 } 01693 01694 if (v_q > 0) // Positive speed values 01695 { 01696 error = -e_d - p_gravity; 01697 p_output_min = p_min; 01698 p_output_max = p_max; 01699 } 01700 else // Negative speed values 01701 { 01702 error = e_d + p_gravity; 01703 p_output_min = -p_max; 01704 p_output_max = -p_min; 01705 } 01706 01707 01708 if(error>p_error_limit) 01709 { 01710 error=p_error_limit; 01711 } 01712 if(error<-p_error_limit) 01713 { 01714 error=-p_error_limit; 01715 } 01716 01717 output = (error * p_kp)>>p_k_scaling; //Kp 01718 01719 comp_ki = (error * p_ki)>>p_k_scaling; // Ki 01720 01721 p_integral += comp_ki; 01722 01723 if (p_integral < p_output_min) 01724 { 01725 p_integral = p_output_min; 01726 } 01727 if (p_integral > p_output_max) 01728 { 01729 p_integral = p_output_max; 01730 } 01731 01732 output += p_integral; 01733 01734 if (output < p_output_min) 01735 { 01736 output = p_output_min; 01737 } 01738 if (output > p_output_max) 01739 { 01740 output = p_output_max; 01741 } 01742 01743 speed = output>>p_reg_scaling; 01744 01745 01747 // Inverse Park Transform - Vector Rotation - Voltage 01749 01750 v_alpha = (((v_d * cos_theta)) - ((v_q * sin_theta)))/128; 01751 v_beta = (((v_d * sin_theta)) + ((v_q * cos_theta)))/128; 01752 01753 01755 // Inverse Clarke Transform - 2 => 3 - Voltage 01757 01758 v_a = v_alpha; 01759 v_b = (-(v_alpha*128)+((v_beta*SQRT_THREE_OVER_TWO)))/256; 01760 v_c = (-(v_alpha*128)-((v_beta*SQRT_THREE_OVER_TWO)))/256; 01761 01762 01764 // ADC Sample 01766 01767 i_sample_2 = ADCA_CH0_RES + i_sample_offset; 01768 ADCA_CH0_MUXCTRL = adc_user_channel; 01769 01771 // Current 2 Sample Calculation 01773 01774 switch (sample_state_2) 01775 { 01776 case 1: // V(001) = I_c 01777 i_c_s = i_sample_2 - HALF_SCALE; 01778 break; 01779 01780 case 2: // V(010) = I_b 01781 i_b_s = i_sample_2 - HALF_SCALE; 01782 break; 01783 01784 case 3: // V(011) = -I_a 01785 i_a_s = HALF_SCALE - i_sample_2; 01786 break; 01787 01788 case 4: // V(100) = I_a 01789 i_a_s = i_sample_2 - HALF_SCALE; 01790 break; 01791 01792 case 5: // V(101) = -I_b 01793 i_b_s = HALF_SCALE - i_sample_2; 01794 break; 01795 01796 case 6: // V(110) = -I_c 01797 i_c_s = HALF_SCALE - i_sample_2; 01798 break; 01799 } 01800 01801 01803 // Three phase current calculation 01805 01806 if ((sample_state_1 != 3) && (sample_state_1 != 4)) 01807 { 01808 if ((sample_state_2 != 3) && (sample_state_2 != 4)) 01809 { 01810 i_a_s = -(i_c_s + i_b_s); 01811 } 01812 } 01813 if ((sample_state_1 != 2) && (sample_state_1 != 5)) 01814 { 01815 if ((sample_state_2 != 2) && (sample_state_2 != 5)) 01816 { 01817 i_b_s = -(i_c_s + i_a_s); 01818 } 01819 } 01820 if ((sample_state_1 != 1) && (sample_state_1 != 6)) 01821 { 01822 if ((sample_state_2 != 1) && (sample_state_2 != 6)) 01823 { 01824 i_c_s = -(i_a_s + i_b_s); 01825 } 01826 } 01827 01828 01830 // PWM State and Sample windows Calculation 01832 01833 pwm_a = PWM_HALF + v_a; 01834 pwm_b = PWM_HALF + v_b; 01835 pwm_c = PWM_HALF + v_c; 01836 01837 pwm_state = 0; 01838 if (pwm_a >= PWM_HALF) 01839 { 01840 pwm_state = 4; 01841 } 01842 if (pwm_b >= PWM_HALF) 01843 { 01844 pwm_state |= 2; 01845 } 01846 if (pwm_c >= PWM_HALF) 01847 { 01848 pwm_state |= 1; 01849 } 01850 01851 switch (pwm_state) 01852 { 01853 case 1: // V(001) 01854 sample_state_1 = 1; // V(001) = I_c 01855 if (pwm_a < pwm_b) 01856 { 01857 sample_state_2 = 3; // V(011) = -I_a 01858 window_1 = pwm_c - pwm_b; 01859 window_2 = pwm_b - pwm_a; 01860 pwm_1 = 1; // first sample 4 = A, 2 = B, 1 = C 01861 pwm_2 = 2; // second sample 4 = A, 2 = B, 1 = C 01862 pwm_3 = 4; // second sample 4 = A, 2 = B, 1 = C 01863 } 01864 else 01865 { 01866 sample_state_2 = 5; // V(101) = -I_b 01867 window_1 = pwm_c - pwm_a; 01868 window_2 = pwm_a - pwm_b; 01869 pwm_1 = 1; // first sample 4 = A, 2 = B, 1 = C 01870 pwm_2 = 4; // second sample 4 = A, 2 = B, 1 = C 01871 pwm_3 = 2; // second sample 4 = A, 2 = B, 1 = C 01872 } 01873 break; 01874 case 2: // V(010) 01875 sample_state_1 = 2; // V(010) = I_b 01876 pwm_1 = 2; // first sample 4 = A, 2 = B, 1 = C 01877 if (pwm_c < pwm_a) 01878 { 01879 sample_state_2 = 6; // V(110) = -I_c 01880 window_1 = pwm_b - pwm_a; 01881 window_2 = pwm_a - pwm_c; 01882 pwm_1 = 2; // first sample 4 = A, 2 = B, 1 = C 01883 pwm_2 = 4; // second sample 4 = A, 2 = B, 1 = C 01884 pwm_3 = 1; // second sample 4 = A, 2 = B, 1 = C 01885 } 01886 else 01887 { 01888 sample_state_2 = 3; // V(011) = -I_a 01889 window_1 = pwm_b - pwm_c; 01890 window_2 = pwm_c - pwm_a; 01891 pwm_1 = 2; // first sample 4 = A, 2 = B, 1 = C 01892 pwm_2 = 1; // second sample 4 = A, 2 = B, 1 = C 01893 pwm_3 = 4; // second sample 4 = A, 2 = B, 1 = C 01894 } 01895 break; 01896 01897 case 4: // V(100) 01898 sample_state_1 = 4; // V(100) = I_a 01899 if (pwm_b < pwm_c) 01900 { 01901 sample_state_2 = 5; // V(101) = -I_b 01902 window_1 = pwm_a - pwm_c; 01903 window_2 = pwm_c - pwm_b; 01904 pwm_1 = 4; // first sample 4 = A, 2 = B, 1 = C 01905 pwm_2 = 1; // second sample 4 = A, 2 = B, 1 = C 01906 pwm_3 = 2; // second sample 4 = A, 2 = B, 1 = C 01907 } 01908 else 01909 { 01910 sample_state_2 = 6; // V(110) = -I_c 01911 window_1 = pwm_a - pwm_b; 01912 window_2 = pwm_b - pwm_c; 01913 pwm_1 = 4; // first sample 4 = A, 2 = B, 1 = C 01914 pwm_2 = 2; // second sample 4 = A, 2 = B, 1 = C 01915 pwm_3 = 1; // second sample 4 = A, 2 = B, 1 = C 01916 } 01917 break; 01918 01919 case 6: // V(110) 01920 sample_state_2 = 6; // V(110) = -I_c 01921 if (pwm_a > pwm_b) 01922 { 01923 sample_state_1 = 4; // V(100) = I_a 01924 window_1 = pwm_a - pwm_b; 01925 window_2 = pwm_b - pwm_c; 01926 pwm_1 = 4; // first sample 4 = A, 2 = B, 1 = C 01927 pwm_2 = 2; // second sample 4 = A, 2 = B, 1 = C 01928 pwm_3 = 1; // second sample 4 = A, 2 = B, 1 = C 01929 } 01930 else 01931 { 01932 sample_state_1 = 2; // V(010) = I_b 01933 window_1 = pwm_b - pwm_a; 01934 window_2 = pwm_a - pwm_c; 01935 pwm_1 = 2; // first sample 4 = A, 2 = B, 1 = C 01936 pwm_2 = 4; // second sample 4 = A, 2 = B, 1 = C 01937 pwm_3 = 1; // second sample 4 = A, 2 = B, 1 = C 01938 } 01939 break; 01940 01941 case 5: // V(101) 01942 sample_state_2 = 5; // V(101) = -I_b 01943 if (pwm_c > pwm_a) 01944 { 01945 sample_state_1 = 1; // V(001) = I_c 01946 window_1 = pwm_c - pwm_a; 01947 window_2 = pwm_a - pwm_b; 01948 pwm_1 = 1; // first sample 4 = A, 2 = B, 1 = C 01949 pwm_2 = 4; // second sample 4 = A, 2 = B, 1 = C 01950 pwm_3 = 2; // second sample 4 = A, 2 = B, 1 = C 01951 } 01952 else 01953 { 01954 sample_state_1 = 4; // V(100) = I_a 01955 window_1 = pwm_a - pwm_c; 01956 window_2 = pwm_c - pwm_b; 01957 pwm_1 = 4; // first sample 4 = A, 2 = B, 1 = C 01958 pwm_2 = 1; // second sample 4 = A, 2 = B, 1 = C 01959 pwm_3 = 2; // second sample 4 = A, 2 = B, 1 = C 01960 } 01961 break; 01962 01963 case 3: // V(011) 01964 sample_state_2 = 3; // V(011) = -I_a 01965 if (pwm_b > pwm_c) 01966 { 01967 sample_state_1 = 2; // V(010) = I_b 01968 window_1 = pwm_b - pwm_c; 01969 window_2 = pwm_c - pwm_a; 01970 pwm_1 = 2; // first sample 4 = A, 2 = B, 1 = C 01971 pwm_2 = 1; // second sample 4 = A, 2 = B, 1 = C 01972 pwm_3 = 4; // second sample 4 = A, 2 = B, 1 = C 01973 } 01974 else 01975 { 01976 sample_state_1 = 1; // V(001) = I_c 01977 window_1 = pwm_c - pwm_b; 01978 window_2 = pwm_b - pwm_a; 01979 pwm_1 = 1; // first sample 4 = A, 2 = B, 1 = C 01980 pwm_2 = 2; // second sample 4 = A, 2 = B, 1 = C 01981 pwm_3 = 4; // second sample 4 = A, 2 = B, 1 = C 01982 } 01983 break; 01984 01985 default: 01986 01987 break; 01988 } 01989 01990 correction_1 = 0; 01991 correction_2 = 0; 01992 if (window_1 < SAMPLE_WINDOW) 01993 { 01994 correction_1 = SAMPLE_WINDOW - window_1; 01995 } 01996 if (window_2 < SAMPLE_WINDOW) 01997 { 01998 correction_2 = SAMPLE_WINDOW - window_2; 01999 } 02000 02001 02003 // Sample 1 PWM "anti-correction" 02005 02006 switch (pwm_1) 02007 { 02008 case 1: // c (001) 1 02009 pwm_a_comp = pwm_a; 02010 pwm_b_comp = pwm_b; 02011 pwm_c_comp = pwm_c - correction_1; 02012 break; 02013 02014 case 2: // b (010) 2 02015 pwm_a_comp = pwm_a; 02016 pwm_b_comp = pwm_b - correction_1; 02017 pwm_c_comp = pwm_c; 02018 break; 02019 02020 case 4: // a (100) 4 02021 pwm_a_comp = pwm_a - correction_1; 02022 pwm_b_comp = pwm_b; 02023 pwm_c_comp = pwm_c; 02024 break; 02025 02026 default: 02027 pwm_a_comp = PWM_HALF; 02028 pwm_b_comp = PWM_HALF; 02029 pwm_c_comp = PWM_HALF; 02030 break; 02031 } 02032 02033 02034 switch (pwm_2) 02035 { 02036 case 1: // c (001) 1 02037 sample_trigger = pwm_c_comp + SAMPLE_1_OFFSET; 02038 break; 02039 02040 case 2: // b (010) 2 02041 sample_trigger = pwm_b_comp + SAMPLE_1_OFFSET; 02042 break; 02043 02044 case 4: // a (100) 4 02045 sample_trigger = pwm_a_comp + SAMPLE_1_OFFSET; 02046 break; 02047 02048 default: 02049 sample_trigger = PWM_HALF; 02050 break; 02051 } 02052 02053 break; 02054 02055 default: 02056 02057 break; 02058 02059 } 02060 02061 // TEST_pin_lo; 02062 02063 } 02064 02065 02066 //***************************************************************************** 02067 // Main 02068 //***************************************************************************** 02069 02077 int main(void) 02078 { 02079 init_oscillator(); 02080 init_test_pin(); 02081 init_pwm(); 02082 init_adc(); 02083 init_uart(); 02084 init_eventsys(); 02085 init_interrupts(); 02086 read_eeprom(); // Read the EEPROM contents once on power up 02087 calibrate_current(); 02088 read_oscillator_cal(); 02089 write_oscillator_cal(); 02090 02091 while(1) 02092 { 02093 //Application code here 02094 } 02095 }
1.8.0